﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.Collections;
using Bling.Util;
using Bling.DSL;
using Bling.Core;
using Bling.Vecs;
using Bling.Matrices;

namespace Bling.Core {
  using Bling.Angles;

  public partial interface IDouble<BRAND, D> : IDouble<BRAND>
    where BRAND : IDouble<BRAND, D>
    where D : Dim<D> {
    //RadianBl<D> Asin { get; }
    //RadianBl<D> Acos { get; }
    //RadianBl<D> Atan { get; }
  }
  public abstract partial class RealDoubleBl<T, BRAND, D> : DoubleBl<T, BRAND, D>
    where BRAND : RealDoubleBl<T, BRAND, D>
    where D : Dim<D> {

    public partial class AsinOp : CallUnaryOp<AsinOp>, IMathLinqOperator {
      public new static readonly AsinOp Instance = new AsinOp();
      private AsinOp() { }
      public override string Name { get { return "Asin"; } }
      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return DoubleBl.AsinOp.Instance.Bl(); }
      }
      public override Expr<IVec<double, D>> Inverse(Expr<IVec<double, D>> argA, Expr<IVec<double, D>> result) {
        return new RadianBl<D>(result).Sin.UnderlyingD;
      }
    }
    public partial class AcosOp : CallUnaryOp<AcosOp>, IMathLinqOperator {
      public new static readonly AcosOp Instance = new AcosOp();
      private AcosOp() { }
      public override string Name { get { return "Acos"; } }
      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return DoubleBl.AcosOp.Instance.Bl(); }
      }
      public override Expr<IVec<double, D>> Inverse(Expr<IVec<double, D>> argA, Expr<IVec<double, D>> result) {
        return new RadianBl<D>(result).Cos.UnderlyingD;
      }
    }
    public partial class AtanOp : CallUnaryOp<AtanOp>, IMathLinqOperator {
      public new static readonly AtanOp Instance = new AtanOp();
      private AtanOp() { }
      public override string Name { get { return "Atan"; } }
      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return DoubleBl.AtanOp.Instance.Bl(); }
      }
      public override Expr<IVec<double, D>> Inverse(Expr<IVec<double, D>> argA, Expr<IVec<double, D>> result) {
        return new RadianBl<D>(result).Tan.UnderlyingD;
      }
    }
    public partial class Atan2Op : CallBinaryOp<Atan2Op>, IMathLinqOperator {
      public new static readonly Atan2Op Instance = new Atan2Op();
      private Atan2Op() { }
      public override string Name { get { return "Atan2"; } }
      public override Func<Expr<double>, Expr<double>, Expr<double>> Uniform {
        get { return DoubleBl.Atan2Op.Instance.Bl(); }
      }
    }


    public RadianBl<D> Asin { get { return new RadianBl<D>(AsinOp.Instance.Make(UnderlyingD)); } }
    public RadianBl<D> Acos { get { return new RadianBl<D>(AcosOp.Instance.Make(UnderlyingD)); } }
    public RadianBl<D> Atan { get { return new RadianBl<D>(AtanOp.Instance.Make(UnderlyingD)); } }

    public BRAND SinU { get { return ToBrandD(PI2.Sin.Provider); } }
    public BRAND CosU { get { return ToBrandD(PI2.Cos.Provider); } }
    public BRAND SinN { get { return ToBrandD(PI.Sin.Provider); } }
    public BRAND CosN { get { return ToBrandD(PI.Cos.Provider); } }

    public RadianBl<D> PI {
      get {
        return (this * Repeat(Math.PI)).Radians;
      }
    }
    public RadianBl<D> PI2 {
      get {
        return (this * Repeat(Math.PI * 2d)).Radians;
      }
    }

    public RadianBl<D> Radians {
      get {
        return new RadianBl<D>(UnderlyingD); 
      }
    }
  }
  public partial class DoubleBl : BaseDoubleBl {
    public Double2Bl SinCosU { get { return PI2.SinCos; } }
    public Double2Bl SinCosN { get { return PI.SinCos; } }

    public new RadianBl Radians {
      get { return new RadianBl(Provider); }
    }
    public new RadianBl PI {
      get {
        return (this * Repeat(Math.PI)).Radians;
      }
    }
    public new RadianBl PI2 {
      get {
        return (this * Repeat(Math.PI * 2d)).Radians;
      }
    }
  }
  public abstract partial class PointDoubleBl<D, BRAND> : FixDoubleBl<D, BRAND>
    where D : Dim1D<D>
    where BRAND : PointDoubleBl<D, BRAND> {
    public abstract BRAND Rotate(BRAND Vector);
    public BRAND RotateBy(IAngle<D> Angle) {
      return ToBrandD(Angle.Transform(this).UnderlyingD);
    }
    public BRAND RotateBy(IAngle<D> Angle, BRAND Center) {
      return Center + (this - Center).RotateBy(Angle);
    }
  }
  public partial class Double2Bl : PointDoubleBl<D2, Double2Bl>, IHasXY<double, DoubleBl> {
    public Double2Bl SinCosU { get { return new Double2Bl(X.SinU,Y.CosU); } }
    public partial class ASinCosOp : UnaryOperatorS<double, ASinCosOp>, INoLinqOperator, INoShaderOperator {
      public new static readonly ASinCosOp Instance = new ASinCosOp();
      private ASinCosOp() { }
      public override string Name { get { return "ASinCos"; } }
      public override Expr<double> Expand(Expr<IVec<double, D2>> argA) { return ToBrandD(argA).ATan2; }
      public override Expr<IVec<double, D2>> Inverse(Expr<IVec<double, D2>> argA, Expr<double> result) {
        return new RadianBl(result).SinCos;
      }
    }
    public RadianBl ASinCos { get { return new RadianBl(ASinCosOp.Instance.Make(Provider)); } }
    public RadianBl ATan2 { get { return new RadianBl(DoubleBl.Atan2Op.Instance.Make(this[0], this[1])); } }

    public RadianBl AngleBetween(Double2Bl p1) {
      Double2Bl p0 = this;
      var a0 = p0.ASinCos - p1.ASinCos;
      return (a0);
    }
    public RadianBl Angle { get { return ASinCos; } }
    public override Double2Bl Rotate(Double2Bl w) {
      (w.IsNormalized).Assert();
      w = this * w.X + this.ClockwisePerp * w.Y;
      return w;
    }
  }
  public partial class Double3Bl : PointDoubleBl<D3, Double3Bl>, IHasXYZ<double, DoubleBl> {
    public class AngleBetweenOp : BinaryOperatorS<IVec<double,D4>,AngleBetweenOp>, Ops.IMemoOperator<IVec<double,D4>>, INoLinqOperator, INoShaderOperator {
      public new static readonly AngleBetweenOp Instance = new AngleBetweenOp();
      private AngleBetweenOp() { }
      public override string Name { get { return "angle"; } }
      public override Expr<IVec<double, D4>> Expand(Expr<IVec<double, D3>> argA, Expr<IVec<double, D3>> argB) {
        Double3Bl v0 = argA;
        Double3Bl v1 = argB;
        var Axis = v1.Cross(v0);
        var Dot = v1.Dot(v0);
        var Angle = Dot.Acos;
        return (Dot >= 1d).Condition(QuaternionBl.Identity, new QuaternionBl(Axis, Angle));
      }
    }
    public QuaternionBl AngleBetween(Double3Bl v1) {
      this.IsNormalized.Assert();
      v1.IsNormalized.Assert();
      QuaternionBl q = AngleBetweenOp.Instance.Make(this, v1);
      return q.VerifyNormalized;
    }
    public override Double3Bl Rotate(Double3Bl v) {
      v.IsNormalized.Assert();
      var p0 = this;
      var p1 = p0.YXZ() * new Double3Bl(-1d, +1d, +1d);
      var p2 = p0.Cross(p1);
      return ((p0 * v.X + p1 * v.Y + p2 * v.Z));
    }

  }
}

namespace Bling.Angles {
  public interface IAngle<P> where P : Dim1D<P> {
    FlexDoubleBl<P> Transform(FlexDoubleBl<P> Point);
  }


  public abstract partial class AngleBl<T, BRAND, D, SCALAR> : DoubleBl<T, BRAND, D>
    where D : Dim<D>
    where BRAND : AngleBl<T, BRAND, D, SCALAR>
    where SCALAR : ValueBrand<double, SCALAR, double, D1, DoubleBl> {
    public AngleBl(Expr<T> Underlying) : base(Underlying) { }
    public AngleBl(Expr<IVec<double, D>> Underlying) : base(Underlying) { }
    public AngleBl(SCALAR[] Es) : base(Es.Select<SCALAR,DoubleBl>(s => s.Provider).ToArray()) { }
    public AngleBl() : base() { }
    public override BRAND Normalize {
      get {
        throw new NotImplementedException();
      }
    }
    public override BRAND RawNormalize {
      get {
        throw new NotImplementedException();
      }
    }
    //public abstract FlexDoubleBl<P> Rotate(FlexDoubleBl<P> Point);
  }
  public abstract partial class Angle2DBl<T, BRAND, D, SCALAR> : AngleBl<T, BRAND, D, SCALAR>
    where D : Dim<D>
    where BRAND : Angle2DBl<T, BRAND, D, SCALAR>
    where SCALAR : ValueBrand<double, SCALAR, double, D1, DoubleBl> {
    public Angle2DBl(Expr<T> Underlying) : base(Underlying) { }
    public Angle2DBl(Expr<IVec<double, D>> Underlying) : base(Underlying) { }
    public Angle2DBl(SCALAR[] Es) : base(Es) { }
    public Angle2DBl() : base() { }

    public FlexDoubleBl<D> Value { get { return new FlexDoubleBl<D>(UnderlyingD); } }

    public static BRAND operator -(Angle2DBl<T, BRAND, D, SCALAR> opA, BRAND opB) {
      return ToBrandD(opA.Value - opB.Value);
    }
    public static BRAND operator *(Angle2DBl<T, BRAND, D, SCALAR> opA, FlexDoubleBl<D> d) {
      return ToBrandD(opA.Value * d);
    }
    public static BRAND operator /(Angle2DBl<T, BRAND, D, SCALAR> opA, FlexDoubleBl<D> d) {
      return ToBrandD(opA.Value / d);
    }

    public abstract FlexDoubleBl<D> Sin { get; }
    public abstract FlexDoubleBl<D> Cos { get; }
    public abstract FlexDoubleBl<D> Tan { get; }
    public abstract FlexDoubleBl<D> Sinh { get; }
    public abstract FlexDoubleBl<D> Cosh { get; }
    public abstract FlexDoubleBl<D> Tanh { get; }

    public static BRAND MaxAngle { get; protected set; }

    public override BRAND Normalize {
      get {
        if (IsNormalized) return this;
        var value = this.Value % MaxAngle;
        value = (value < 0).Condition(MaxAngle + value, value);
        return ToBrandD(value.Provider).VerifyNormalized;
      }
    }
    public override BRAND RawNormalize {
      get {
        return this.Normalize;
      }
    }
  }

  public static class AngleExtensions {

    /// <summary>
    /// Convert number into the radian angle d * Math.PI.
    /// </summary>
    public static RadianBl PI(this DoubleBl d) { return new RadianBl(d * Math.PI); }
    /// <summary>
    /// Convert number into the radian angle d * Math.PI.
    /// </summary>
    public static RadianBl PI(this double d) { return d.Bl().PI(); }
    /// <summary>
    /// Convert number into the radian angle d * Math.PI.
    /// </summary>
    public static RadianBl PI(this int d) { return ((DoubleBl)d).PI(); }
    /// <summary>
    /// Convert number into the radian angle d * Math.PI.
    /// </summary>
    public static RadianBl PI(this IntBl d) { return ((DoubleBl)d).PI(); }

    /// <summary>
    /// Convert number into degree angle.
    /// </summary>
    public static DegreeBl Degrees(this double d) { return new DegreeBl(((DoubleBl)d).Provider); }
    /// <summary>
    /// Convert number into degree angle.
    /// </summary>
    public static DegreeBl Degrees(this int d) { return new DegreeBl(((DoubleBl)d).Provider); }
    /// <summary>
    /// Convert number into degree angle.
    /// </summary>
    public static DegreeBl Degrees(this DoubleBl d) { return new DegreeBl(d); }
    /// <summary>
    /// Convert number into radian angle.
    /// </summary>
    public static RadianBl Radians(this double d) { return new RadianBl(d.Bl()); }
    /// <summary>
    /// Convert number into radian angle.
    /// </summary>
    public static RadianBl Radians(this int d) { return new RadianBl(d.Bl()); }
    //public static RadianBl Radians(this Expr<double> d) { return new RadianBl(d); }


  }


  public abstract partial class RadianBl<T, BRAND, D> : Angle2DBl<T, BRAND, D, RadianBl>, ICanInterpolate<BRAND>
    where BRAND : RadianBl<T, BRAND, D>
    where D : Dim<D> {
    public RadianBl(Expr<T> Underlying) : base(Underlying) { }
    public RadianBl(Expr<IVec<double, D>> Underlying) : base(Underlying) { }
    public RadianBl(RadianBl[] Es) : base(Es) { }
    public RadianBl() : base() { }

    static RadianBl() {
      MaxAngle = Repeat(2d.PI().Value);
    }


    //public FlexDoubleBl<D> Value { get { return FlexDoubleBl<D>.ToBrandD(UnderlyingD); } }
    public BRAND Inverse { get { return ToBrandD(FlexDoubleBl<D>.Repeat(2d.PI().Value).Subtract(this.Value).UnderlyingD); }  }


    public static BRAND operator +(RadianBl<T, BRAND, D> r0, BRAND r1) {
      return ToBrandD(r0.Value + r1.Value);

    }
    public static FlexDoubleBl<D> operator /(RadianBl<T, BRAND, D> r0, BRAND r1) {
      return (r0.Value / r1.Value);
    }
    public override Func<BRAND, BRAND, DoubleBl, BRAND> CoreInterpolate {
      get {
        return (Self, To, t) => {
          var Delta = To.Value - Self.Value;
          Delta = Delta % (Math.PI * 2d);
          Delta = (Delta.Abs > Math.PI).Condition(Delta.Sign * -1 * (Math.PI * 2d - Delta.Abs), Delta);
          return ToBrandD(Value + Delta * t);
        };
      }
    }

    public partial class SinOp : CallUnaryOp<SinOp>, IMathLinqOperator {
      public new static readonly SinOp Instance = new SinOp();
      private SinOp() { }
      public override string Name { get { return "Sin"; } }
      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return RadianBl.SinOp.Instance.Bl(); }
      }
      public override Expr<IVec<double, D>> Inverse(Expr<IVec<double, D>> argA, Expr<IVec<double, D>> result) {
        return FlexDoubleBl<D>.ToBrandD(result).Asin.UnderlyingD;
      }
    }

    public partial class CosOp : CallUnaryOp<CosOp>, IMathLinqOperator {
      public new static readonly CosOp Instance = new CosOp();
      private CosOp() { }
      public override string Name { get { return "Cos"; } }
      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return RadianBl.CosOp.Instance.Bl(); }
      }
      public override Expr<IVec<double, D>> Inverse(Expr<IVec<double, D>> argA, Expr<IVec<double, D>> result) {
        return FlexDoubleBl<D>.ToBrandD(result).Acos.UnderlyingD;
      }
    }
    public partial class TanOp : CallUnaryOp<TanOp>, IMathLinqOperator {
      public new static readonly TanOp Instance = new TanOp();
      private TanOp() { }
      public override string Name { get { return "Tan"; } }
      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return RadianBl.TanOp.Instance.Bl(); }
      }
      public override Expr<IVec<double, D>> Inverse(Expr<IVec<double, D>> argA, Expr<IVec<double, D>> result) {
        return FlexDoubleBl<D>.ToBrandD(result).Atan.UnderlyingD;
      }
    }
    public partial class SinhOp : CallUnaryOp<SinhOp>, IMathLinqOperator {
      public new static readonly SinhOp Instance = new SinhOp();
      private SinhOp() { }
      public override string Name { get { return "Sinh"; } }
      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return RadianBl.SinhOp.Instance.Bl(); }
      }
    }

    public partial class CoshOp : CallUnaryOp<CoshOp>, IMathLinqOperator {
      public new static readonly CoshOp Instance = new CoshOp();
      private CoshOp() { }
      public override string Name { get { return "Cosh"; } }
      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return RadianBl.CoshOp.Instance.Bl(); }
      }
    }
    public partial class TanhOp : CallUnaryOp<TanhOp>, IMathLinqOperator {
      public new static readonly TanhOp Instance = new TanhOp();
      private TanhOp() { }
      public override string Name { get { return "Tanh"; } }
      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return RadianBl.TanhOp.Instance.Bl(); }
      }
    }
    public override FlexDoubleBl<D> Sin { get { return FlexDoubleBl<D>.ToBrandD(SinOp.Instance.Make(UnderlyingD)); } }
    public override FlexDoubleBl<D> Cos { get { return FlexDoubleBl<D>.ToBrandD(CosOp.Instance.Make(UnderlyingD)); } }
    public override FlexDoubleBl<D> Tan { get { return FlexDoubleBl<D>.ToBrandD(TanOp.Instance.Make(UnderlyingD)); } }
    public override FlexDoubleBl<D> Sinh { get { return FlexDoubleBl<D>.ToBrandD(SinhOp.Instance.Make(UnderlyingD)); } }
    public override FlexDoubleBl<D> Cosh { get { return FlexDoubleBl<D>.ToBrandD(CoshOp.Instance.Make(UnderlyingD)); } }
    public override FlexDoubleBl<D> Tanh { get { return FlexDoubleBl<D>.ToBrandD(TanhOp.Instance.Make(UnderlyingD)); } }

    public override Core.StringBl ToStringBl() {
      return (Value.Divide(FlexDoubleBl<D>.Repeat(Math.PI))).ToStringBl() + "PI";
    }
    public override Core.StringBl ToStringBl(DoubleBl e) {
      return (Value.Divide(FlexDoubleBl<D>.Repeat(Math.PI))).ToStringBl(e) + "PI";
    }

    

  }
  public partial class RadianBl : RadianBl<double, RadianBl, D1>, IAngle<D2> {
    public RadianBl(DoubleBl Underlying) : base(Underlying.Provider) { }
    public RadianBl(Expr<double> Underlying) : base(Underlying) { }
    public RadianBl(Expr<IVec<double, D1>> Underlying) : base(Underlying) { }
    public RadianBl() : base() { }

    public new DoubleBl Value { get { return Provider; } }
    public DoubleBl PI { get { return Value / Math.PI; } }

    public AxisAngle3DBl RotateAround(Double3Bl Axis) { return new AxisAngle3DBl(Axis, this); }

    public static implicit operator RadianBl(Expr<double> u) { return new RadianBl(u); }
    public static implicit operator RadianBl(Expr<IVec<double,D1>> u) { return new RadianBl(u); }
    public static implicit operator RadianBl(RadianBl<D1> r) { return r.Provider; }

    public FlexDoubleBl<D2> Transform(FlexDoubleBl<D2> P) {
      var w = this.SinCos;
      (w.IsNormalized).Assert();
      w = P * w.X + ((Double2Bl) P).ClockwisePerp * w.Y;
      return w;
    }

    public partial class SinCosOp : UnaryOperatorS<IVec<double,D2>,SinCosOp>, ICallOperator, INoLinqOperator, INoShaderOperator {
      public new static readonly SinCosOp Instance = new SinCosOp();
      private SinCosOp() { }
      public override string Name { get { return "SinCos"; } }
      public override Expr<IVec<double, D2>> Expand(Expr<IVec<double, D1>> argA) {
        return new Double2Bl(ToBrandD(argA).Sin, ToBrandD(argA).Cos);
      }
      public override Expr<IVec<double, D1>> Inverse(Expr<IVec<double, D1>> argA, Expr<IVec<double, D2>> result) {
        return new Double2Bl(result).ASinCos;
      }
    }
    public Double2Bl SinCos { get { return ((Double2Bl) SinCosOp.Instance.Make(UnderlyingD)).VerifyNormalized; } }
    public Double2Bl ToPoint { get { return (2.5d.PI() - this).SinCos; } }



    // XXX: ToPoint = (Inverse + .5.PI()).SinCos
    public class DegreesOp : CallUnaryOp<DegreesOp>, INoLinqOperator {
      public new static readonly DegreesOp Instance = new DegreesOp();
      private DegreesOp() { }
      public override string Name { get { return "Degrees"; } }

      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return null; }
      }
      protected override Expr<IVec<double, D1>> Expand0(Expr<IVec<double, D1>> argA) {
        return (ToBrandD(argA).Value * (180.0 / Math.PI));
      }
      public override Expr<IVec<double, D1>> Inverse(Expr<IVec<double, D1>> argA, Expr<IVec<double, D1>> result) {
        return new DegreeBl(result).Radians;
      }
    }
    public DegreeBl Degrees { get { return new DegreeBl(DegreesOp.Instance.Make(UnderlyingD)); } }

    public Matrix3Bl Matrix { get { return RotateAt0(0d); } }
    public Matrix3Bl RotateAt(Double2Bl Center) {
      return Matrix3Bl.MakeTranslate(+Center) * Matrix * Matrix3Bl.MakeTranslate(-Center);
      /*
      var p = Center;
      var delta = (p * (1.0 - Cos)) + (p.YX() * Sin * new Double2Bl(+1, -1));
      return RotateAt0(delta);
       */
    }
    private Matrix3Bl RotateAt0(Double2Bl delta) {
      return new Matrix3Bl(+Cos, +Sin, delta.X,
                           -Sin, +Cos, delta.Y,
                              0,    0,       1).VerifyAffine;
    }
  }
  public partial class RadianBl<D> : RadianBl<IVec<double, D>, RadianBl<D>, D> where D : Dim<D> {
    public RadianBl(Expr<IVec<double, D>> Underlying) : base(Underlying) { }
    public RadianBl(RadianBl[] Es) : base(Es) { }
    public RadianBl() : base() { }
    public static implicit operator RadianBl<D>(RadianBl r) { return RadianBl<D>.Repeat(r.Value); }
    public static implicit operator RadianBl<D>(RadianBl<D1> r) { return RadianBl<D>.Repeat(r.Value); }
  }

  public partial class DegreeBl : Angle2DBl<double, DegreeBl, D1, DegreeBl>, IAngle<D2> {
    public DegreeBl(DoubleBl Underlying) : base(Underlying.Provider) { }
    public DegreeBl(Expr<double> Underlying) : base(Underlying) { }
    public DegreeBl(Expr<IVec<double, D1>> Underlying) : base(Underlying) { }
    public DegreeBl() : base() { }

    static DegreeBl() {
      MaxAngle = 360d.Degrees();
    }
    public FlexDoubleBl<D2> Transform(FlexDoubleBl<D2> P) { return Radians.Transform(P); }

    public new DoubleBl Value { get { return Provider; } }
    public DegreeBl Inverse { get { return new DegreeBl(360d.Bl() - Value); } }

    public class RadiansOp : CallUnaryOp<RadiansOp>, INoLinqOperator {
      public new static readonly RadiansOp Instance = new RadiansOp();
      private RadiansOp() { }
      public override string Name { get { return "Radians"; } }

      public override Func<Expr<double>, Expr<double>> Uniform {
        get { return null; }
      }
      protected override Expr<IVec<double, D1>> Expand0(Expr<IVec<double, D1>> argA) {
        return (ToBrandD(argA).Value / (180.0 / Math.PI));
      }
      public override Expr<IVec<double, D1>> Inverse(Expr<IVec<double, D1>> argA, Expr<IVec<double, D1>> result) {
        return new RadianBl(result).Degrees;
      }
    }
    /// <summary>
    /// Convert this angle into a radian angle. 
    /// </summary>
    public RadianBl Radians { 
      get { 
        return new RadianBl(RadiansOp.Instance.Make(UnderlyingD)); 
      } 
      set { Radians.Bind = value; } 
    }
    public override FlexDoubleBl<D1> Sin { get { return Radians.Sin; } }
    public override FlexDoubleBl<D1> Cos { get { return Radians.Cos; } }
    public override FlexDoubleBl<D1> Tan { get { return Radians.Tan; } }
    public override FlexDoubleBl<D1> Sinh { get { return Radians.Sinh; } }
    public override FlexDoubleBl<D1> Cosh { get { return Radians.Cosh; } }
    public override FlexDoubleBl<D1> Tanh { get { return Radians.Tanh; } }
  }

  public abstract partial class Angle3DBl<T, BRAND, D, SCALAR> : AngleBl<T, BRAND, D, SCALAR>
    where BRAND : Angle3DBl<T, BRAND, D, SCALAR>
    where D : Dim1D<D>
    where SCALAR : NumericBrand<double, SCALAR, double, D1, DoubleBl> {
    public Angle3DBl(Expr<T> Underlying) : base(Underlying) { }
    public Angle3DBl(Expr<IVec<double, D>> Underlying) : base(Underlying) { }
    public Angle3DBl(params SCALAR[] Es) : base(Es) { }
    public Angle3DBl() : base() { }
  }

  public partial class QuaternionBl : Angle3DBl<IVec<double, D4>, QuaternionBl, D4, DoubleBl>, IHasXYZW<double,DoubleBl>, ICanInterpolate<QuaternionBl>, IAngle<D3>, ICanBeAffineMatrix<Matrix4Bl> {
    public QuaternionBl(Expr<IVec<double, D4>> Underlying) : base(Underlying) { }
    public QuaternionBl(DoubleBl X, DoubleBl Y, DoubleBl Z, DoubleBl W) : base(new DoubleBl[] { X, Y, Z, W }) { }
    public QuaternionBl(Double3Bl XYZ, DoubleBl W) : this(XYZ.X, XYZ.Y, XYZ.Z, W) { }
    public QuaternionBl(Double3Bl Axis, RadianBl W) : this(FromAngle(Axis,W,true)) {
      IsNormalized = true;
    }
    public QuaternionBl() : base() { }

    public override Core.StringBl ToStringBl(DoubleBl D) {
      return ((Double4Bl)this).ToStringBl(D);
    }

    public override Func<QuaternionBl, QuaternionBl, DoubleBl, QuaternionBl> CoreInterpolate {
      get { return (Self, To, t) => Self.Slerp(To, t); }
    }
    public static readonly QuaternionBl Identity;
    static QuaternionBl() {
      Identity = new QuaternionBl(0, 0, 0, 1).VerifyNormalized;
      (Identity.IsNormalized).Assert();
        

      //Default = Identity;
    }

    public static implicit operator QuaternionBl(Expr<IVec<double, D4>> v) { return new QuaternionBl(v); }
    public static explicit operator Double4Bl(QuaternionBl v) { return new Double4Bl(v); }
    public static explicit operator QuaternionBl(Double4Bl v) { return new QuaternionBl(v); }


    private static Expr<IVec<double,D4>> FromAngle(Double3Bl Axis, RadianBl Angle, bool IsSafe) {
      DoubleBl m = Axis.Length;
      Double2Bl a = (Angle / 2d).SinCos;
      Double3Bl XYZ = a.X * (Axis / m); // axis * sin(theta / 2)
      DoubleBl W = a.Y; // cos(theta / 2)

      if (IsSafe) return new QuaternionBl(XYZ, W).VerifyNormalized;
      else return (m > 0.0001).Condition(new QuaternionBl(XYZ, W).VerifyNormalized, Identity);
    }
    protected DoubleBl BaseLength { get { return ((Double4Bl)this).Length; } }
    public override DoubleBl Length {
      get {
        var length0 = BaseLength;
        var max = this.Abs.Max();
        return length0.IsInfinity.Condition((this / max).BaseLength * max, length0);
      }
    }
    protected new Double4Bl Abs { get { return (((Double4Bl)this).Abs); } }

    public override QuaternionBl Normalize {
      get {
        if (IsNormalized) return this;
        DoubleBl m = Length;
        var ret = (m > 0.0001).Condition(RawNormalize, Identity);
        return ret.VerifyNormalized;
      }
    }

    public override QuaternionBl RawNormalize {
      get {
        return this / Length;
      }
    }
    public override QuaternionBl ChooseA(BoolBl Test, QuaternionBl Other) {
      var ret = base.ChooseA(Test, Other);
      if (IsNormalized && Other.IsNormalized) ret = ret.VerifyNormalized;
      return ret;
    }
    public static QuaternionBl operator /(QuaternionBl q, DoubleBl d) {
      return (QuaternionBl)(((Double4Bl)q) / d);
    }
    public static QuaternionBl operator *(QuaternionBl q, DoubleBl d) {
      return (QuaternionBl)(((Double4Bl)q) * d);
    }
    public static QuaternionBl operator +(QuaternionBl qA, QuaternionBl qB) {
      return (QuaternionBl)(((Double4Bl)qA) + ((Double4Bl)qB));
    }
    public static QuaternionBl operator -(QuaternionBl qA) {
      return (QuaternionBl)(((Double4Bl)qA) * -1d);
    }
    public static QuaternionBl operator /(QuaternionBl qA, QuaternionBl qB) {
      return qA.Divide(qB);
    }
    public static QuaternionBl operator *(QuaternionBl qA, QuaternionBl qB) {
      return qA.Multiply(qB);
    }

    public class ToMatrixOp : Ops.Operator<IVec<double,D4>, IVec<double, Dim<D4,D4>>, ToMatrixOp>, Ops.IMemoOperator<IVec<double, Dim<D4,D4>>> {
      public new static readonly ToMatrixOp Instance = new ToMatrixOp();
      private ToMatrixOp() { }
      public override SmartString Format(SmartString argA) { return argA + ".matrix"; }
      public override Expr<IVec<double, Dim<D4, D4>>> Expand(Expr<IVec<double, D4>> argA) {
        var Self = ToBrandD(argA);
        var mA = new DoubleBl[4, 4];
        var mB = new DoubleBl[4, 4];
        Func<int, int> Ck = i => (i < 0) ? 4 + i : i % 4;

        for (int col = 0; col < 4; col++) {
          for (int row = 0; row < 4; row++) {
            if (col == row) {
              (((object)mA[row, col]) == null).Assert();
              mA[row, col] = Self.W;
              mB[row, col] = Self.W;
            }
            if (Ck(4 - (col - 1)) == row) {
              (((object)mA[row, col]) == null).Assert();
              mA[row, col] = Self.Z * (row == 0 || row == 3 ? -1 : +1);
              mB[row, col] = Self.Z * (row % 2 == 0 ? -1 : +1);
            }
            if (Ck(col - 2) == row) {
              (((object)mA[row, col]) == null).Assert();
              mA[row, col] = Self.Y * (row <= 1 ? +1 : -1);
              mB[row, col] = Self.Y * (row == 0 || row == 3 ? +1 : -1);
            }
            if (3 - col == row) {
              (((object)mA[row, col]) == null).Assert();
              mA[row, col] = Self.X * (row % 2 == 0 ? +1 : -1);
              mB[row, col] = Self.X * (row <= 1 ? -1 : +1);
            }
            (((object)mA[row, col]) != null).Assert();
          }
        }
        return new Matrix4Bl(mA) * new Matrix4Bl(mB);
      }
    }
    public Matrix4Bl Matrix {
      get {
        (this.IsNormalized).Assert();
        return ((Matrix4Bl) ToMatrixOp.Instance.Make(this.Provider)).VerifyAffine; }
    }
    public QuaternionBl Slerp(QuaternionBl Other, DoubleBl t) {
      QuaternionBl p = this;
      QuaternionBl q = Other;

      DoubleBl eps = 1e-7;
      q = (p.Distance(q) > p.Distance(q)).Condition(-q, q);

      QuaternionBl qtAB, qtC;
      var cosHalfTheta = p.Dot(q);
      {
        var halfTheta = cosHalfTheta.Acos; 
        var sinHalfTheta = halfTheta.Sin;

        RadianBl<D2> ratioAngle = halfTheta;
        ratioAngle = ratioAngle * new Double2Bl(1.0 - t, 0.0 + t);
        Double2Bl ratioXY = ratioAngle.Sin / sinHalfTheta;
        Double2Bl alt = new Double2Bl(1.0 - t, 0.0 + t);
        Double2Bl ratio = ((1.0d - cosHalfTheta) > eps).Condition(ratioXY, alt);
        qtAB = p * ratio.X + q * ratio.Y;
      }
      {
        RadianBl<D2> ratioAngle = .5.PI();
        DoubleBl ratioX = (.5.PI() * (1.0 - t)).Sin;
        DoubleBl ratioY = (.5.PI() * (0.0 + t)).Sin;
        qtC = (p * ratioX + q * ratioY);
      }
      return ((1.0 + cosHalfTheta) > eps).Condition(qtAB, qtC).VerifyNormalized;
    }


    public new class MultiplyOp : BinaryOp<MultiplyOp>, Ops.IMemoOperator<IVec<double,D4>>, INoShaderOperator, INoLinqOperator {
      public new static readonly MultiplyOp Instance = new MultiplyOp();
      private MultiplyOp() { }
      public override Func<Expr<double>, Expr<double>, Expr<double>> Uniform {
        get { return null; }
      }
      public override string Name {
        get { return "*"; }
      }
      protected override Expr<IVec<double, D4>> Expand0(Expr<IVec<double, D4>> argA, Expr<IVec<double, D4>> argB) {
        QuaternionBl q = ToBrandD(argA);
        QuaternionBl r = ToBrandD(argB);
        var W = q.W * r.W - q.XYZ().Dot(r.XYZ());
        var XYZ = q.W * r.XYZ() +
                  r.W * q.XYZ() +
                  q.YZX() * r.ZXY() -
                  r.YZX() * q.ZXY();
        return new QuaternionBl(XYZ, W);
      }
      public override Expr<IVec<double, D4>> InverseA(Expr<IVec<double, D4>> argA, Expr<IVec<double, D4>> argB, Expr<IVec<double, D4>> result) {
        return DivideOp.Instance.Make(result, argB);
      }
    }
    public new class DivideOp : BinaryOp<DivideOp>, Ops.IMemoOperator<IVec<double, D4>>, INoShaderOperator, INoLinqOperator {
      public new static readonly DivideOp Instance = new DivideOp();
      private DivideOp() { }
      public override Func<Expr<double>, Expr<double>, Expr<double>> Uniform {
        get { return null; }
      }
      public override string Name {
        get { return "/"; }
      }
      protected override Expr<IVec<double, D4>> Expand0(Expr<IVec<double, D4>> argA, Expr<IVec<double, D4>> argB) {
        QuaternionBl q = ToBrandD(argA);
        QuaternionBl r = ToBrandD(argB);
        var W = q.XYZ().Dot(r.XYZ());
        var XYZ = r.W * q.XYZ() -
                  q.W * r.XYZ() -
                  r.YZX() * q.ZXY() +
                  q.YZX() * r.ZXY();

        var L2 = 1d; // r.LengthSquared;
        return new QuaternionBl(XYZ / L2, W / L2);
      }
      public override Expr<IVec<double, D4>> InverseA(Expr<IVec<double, D4>> argA, Expr<IVec<double, D4>> argB, Expr<IVec<double, D4>> result) {
        return MultiplyOp.Instance.Make(result, argB);
      }
    }

    public override QuaternionBl Multiply(QuaternionBl OpB) {
      if (OpB.Provider.Equals(Identity.Provider)) return this;
      if (Provider.Equals(Identity.Provider)) return OpB;
      QuaternionBl ret = (MultiplyOp.Instance.Make(this, OpB));
      return ret.VerifyNormalized;
    }
    public override QuaternionBl Divide(QuaternionBl OpB) {
      return (DivideOp.Instance.Make(this, OpB));
    }
    public Double3Bl Axis {
      get {
        return this.XYZ().Normalize;
      }
    }
    public RadianBl Angle {
      get {
        var msin = this.XYZ().Length;
        var mcos = this.W;
        var maxcoeff = this.XYZ().Abs.Max();
        var msin0 = (this.XYZ() / maxcoeff).Length;
        var mcos0 = this.W / maxcoeff;
        msin = msin.IsInfinity.Condition(msin0, msin);
        mcos = msin.IsInfinity.Condition(mcos0, mcos);
        return new Double2Bl(msin,mcos).ASinCos * 2d;
      }
    }
    public QuaternionBl Conjugate {
      get {
        return new QuaternionBl(this.XYZ() * -1d, W).VerifyNormalized;
      }
    }
    public QuaternionBl Inverse {
      get {
        (IsNormalized).Assert();
        var ret = Conjugate;
        return ret;
      }
    }

    private class TransformOp : Ops.Operator<IVec<double,D4>, IVec<double,D3>, IVec<double,D3>, TransformOp>, Ops.IMemoOperator<IVec<double,D3>> {
      public new static readonly TransformOp Instance = new TransformOp();
      private TransformOp() { }
      public override SmartString Format(SmartString argA, SmartString argB) { return argA + ".transform[" + argB + "]"; }
      public override Expr<IVec<double, D3>> Expand(Expr<IVec<double, D4>> argA, Expr<IVec<double, D3>> argB) {
        var q = ToBrandD(argA);
        var m = -q.XYZ().Square;
        m = m.Sw<Y,X,X>() + m.Sw<Z,Z,Y>();

        var n = q.W * q.XYZ();
        var r = q.Sw<X,X,Y>() * q.Sw<Y,Z,Z>();
        var t = r.Sw<X,Z,Y>() - n.Sw<Z,X,Y>();
        var u = n.Sw<Y,Z,X>() + r.Sw<Y,X,Z>();
        Double3Bl p = argB;
        var v = m * p.XYZ() + t * p.YZX() + u * p.ZXY();
        return 2d * v + p.XYZ();
      }
    }
    public FlexDoubleBl<D3> Transform(FlexDoubleBl<D3> P) {
      return Transform((Double3Bl)P);
    }

    public Double3Bl Transform(Double3Bl p) {
      var ret = ((Double3Bl) TransformOp.Instance.Make(this, p.Normalize)) * p.Length;
      // rotation preserves normalization
      if (p.IsNormalized) ret = ret.VerifyNormalized;
      return ret;
    }
    private Double3Bl NM(Double3Bl d) {
      return d;
    }
  }

  public partial class AxisAngle3DBl : Angle3DBl<IVec<double, D4>, AxisAngle3DBl, D4, DoubleBl>, IHasXYZW<double, DoubleBl>, ICanBeAffineMatrix<Matrix4Bl> {
    public AxisAngle3DBl(Expr<IVec<double, D4>> Underlying) : base(Underlying) { }
    public AxisAngle3DBl() : base() { }
    public AxisAngle3DBl(Double3Bl Axis, RadianBl Angle)
      : base(Axis.X, Axis.Y, Axis.Z, Angle.Value) {
      Axis.IsNormalized.Assert();
    }
    public Double3Bl Axis {
      get { return this.XYZ(); }
    }
    public RadianBl Angle { get { return this.W.Radians; } }
    /// <summary>
    /// Convert 3D axis angle into a 4x4 matrix.
    /// </summary>
    public Matrix4Bl Matrix {
      get {
        var u = this.Axis;
        var c = this.Angle.Cos;
        var s = this.Angle.Sin;
        var n = new DoubleBl[4, 4];


        n[0, 0] = u.X.Square + (1d - u.X.Square) * c;
        n[1, 0] = u.X * u.Y * (1 - c) - u.Z * s;
        n[2, 0] = u.X * u.Z * (1 - c) + u.Y * s;

        n[0, 1] = u.X * u.Y * (1 - c) + u.Z * s;
        n[1, 1] = u.Y.Square + (1d - u.Y.Square) * c;
        n[2, 1] = u.Y * u.Z * (1 - c) - u.X * s;

        n[0, 2] = u.X * u.Z * (1 - c) - u.Y * s;
        n[1, 2] = u.Y * u.Z * (1 - c) + u.X * s;
        n[2, 2] = u.Z.Square + (1d - u.Z.Square) * c;
        n[3, 3] = 1;
        for (int i = 0; i < 3; i++) {
          n[3, i] = 0;
          n[i, 3] = 0;
        }
        return new Matrix4Bl(n).VerifyAffine;

      }
    }
  }
}